/**
 * Copyright (c) 2024, https://www.zhz.com/
 * All rights reserved.
 * $Revision: 1.0 $
 * $Date: 2024/06/14 20:33:40 $
 * $Author: MSun $
 ******************************************************************************/

#ifndef ZHZ_DRIVER_CAMERA_H
#define ZHZ_DRIVER_CAMERA_H
// #pragma once

#include <iostream>
#include <string>
#include <sstream>
#include <opencv2/core.hpp>
#include <opencv2/highgui.hpp>
#include <opencv2/videoio.hpp>
#include <opencv2/opencv.hpp>
#include <opencv2/calib3d.hpp> // 用于SGBM
#include <opencv2/imgproc/imgproc.hpp>


#include <ros/ros.h>
#include <ros/console.h>
#include <cv_bridge/cv_bridge.h>
#include <image_transport/image_transport.h>
#include <sensor_msgs/image_encodings.h>
// 同步订阅
// #include <message_filters/subscriber.h>
// #include <message_filters/time_synchronizer.h>
// #include <message_filters/sync_policies/approximate_time.h>


namespace zhz {

class DriverCamera {
 private:

  ros::ServiceServer single_image_analysis_service_;

  // ros::Publisher image_publisher_;
  ros::NodeHandle nh_;
  image_transport::ImageTransport img_nh_;
  ros::Publisher camera_topic_pub_;

  image_transport::Publisher image_publisher_;
  ros::Publisher  camera_info_pub_l_;
  image_transport::CameraPublisher image_pub_l_;
  image_transport::CameraPublisher image_pub_r_;
  image_transport::Publisher image_pub_l_rect_;
  image_transport::Publisher image_pub_r_rect_;
  image_transport::Subscriber image_sub_;


  bool use_webcam_;
  std::string video_file_;
  bool HNY_CV_002;
  int CAMERA_INDEX;
  int image_width_;
  int image_height_;
  std::string camera_param_;

  cv::Mat camera_intrinsic_l;
  cv::Mat camera_extrinsic_l;
  cv::Mat camera_distcoeff_l;

  cv::Mat camera_intrinsic_r;
  cv::Mat camera_extrinsic_r;
  cv::Mat camera_distcoeff_r;
  cv::Mat RotMatL2R;
  cv::Mat TransMatL2R;

  cv::Size image_size_;
  sensor_msgs::CameraInfo cam_info_l_;
  sensor_msgs::CameraInfo cam_info_r_;
  


 public:
  DriverCamera(ros::NodeHandle& nh, ros::NodeHandle& pnh);
  ~DriverCamera();
  void Init();
  void Run();
  void GetCameraInfo(sensor_msgs::CameraInfo &cam);
  void ImageCapture();
  void ImageProcess(const sensor_msgs::ImageConstPtr& msg);
  int pub_rate_;
  int use_right_camera_;
  std::string camera_topic_;
  int pub_camera_info_;

};

} // namespace zhz

#endif // ZHZ_DRIVER_CAMERA_H